Kalman Equation Reference

We're just including this here in case you want to refer back to the Kalman Filter equations at any time. Feel free to move along :)

Variable Definitions

\mathbf{\hat{x}} - state vector

\mathbf{F} - state transition matrix

\mathbf{P} - error covariance matrix

\mathbf{Q} - process noise covariance matrix

\mathbf{R} - measurement noise covariance matrix

\mathbf{S} - intermediate matrix for calculating Kalman gain

\mathbf{K} - Kalman gain

\mathbf{\tilde{y}} - difference between predicted state and measured state

\mathbf{z} - measurement vector (lidar data or radar data, etc.)

\mathbf{I} - Identity matrix

Prediction Step Equations

PREDICT STATE VECTOR AND ERROR COVARIANCE MATRIX

\mathbf{\hat{x}{k|k-1}} = \mathbf{F{k}} \mathbf{\hat{x}_{k-1|k-1}}

\mathbf{P_{k|k-1}} = \mathbf{F_{k}} \mathbf{P_{k-1|k-1}} \mathbf{F_{k}^T} + \mathbf{Q_{k}}

Update Step Equations

KALMAN GAIN

\mathbf{S_{k}} = \mathbf{H_{k}} \mathbf{P_{k|k-1}} \mathbf{H_{k}^T} + \mathbf{R_{k}}

\mathbf{K_{k}} = \mathbf{P_{k|k-1}} \mathbf{H_{k}^T} \mathbf{S_{k}}^{-1}

UPDATE STATE VECTOR AND ERROR COVARIANCE MATRIX

\mathbf{\tilde{y_{k}}} = \mathbf{z_{k}} - \mathbf{H_{k}} \mathbf{\hat{x}_{k|k-1}}

\mathbf{\hat{x}{k|k}} = \mathbf{\hat{x}{k|k-1}} +\mathbf{ K_{k}} \mathbf{\tilde{y_{k}}}

\mathbf{P_{k|k}} = (\mathbf{I} - \mathbf{ K_{k}} \mathbf{H_{k}}) \mathbf{P_{k|k-1}}